
#include <os/mm.h>
#include <os/lock.h>
#include <os/sched.h>
#include <os/time.h>
#include <os/irq.h>
#include <screen.h>
#include <stdio.h>
#include <assert.h>
#include <os/queue.h>
extern queue_t sleep_queue;
extern queue_t ready_queue[3];
extern queue_t * current_queue;
pcb_t pcb[NUM_MAX_TASK];
const ptr_t pid0_stack = INIT_KERNEL_STACK + PAGE_SIZE;
pcb_t pid0_pcb = {
    .pid = 0,
    .kernel_sp = (ptr_t)pid0_stack,
    .user_sp = (ptr_t)pid0_stack,
    .preempt_count = 0
};



/* current running task PCB */
pcb_t * volatile current_running;

/* global process id */
pid_t process_id = 1;
typedef pcb_t item_t;
void *queue_dequeue(queue_t *queue)
{
    item_t *temp = (item_t *)queue->head;

    /* this queue only has one item */
    if (temp->next == NULL)
    {
        queue->head = queue->tail = NULL;
    }
    else
    {
        queue->head = ((item_t *)(queue->head))->next;
        ((item_t *)(queue->head))->prev = NULL;
    }

    temp->prev = NULL;
    temp->next = NULL;

    return (void *)temp;
}
//queue related
void queue_init(queue_t *queue)
{
    queue->head = queue->tail = NULL;
}

int queue_is_empty(queue_t *queue)
{
    if (queue->head == NULL)
    {
        return 1;
    }
    return 0;
}

void queue_push(queue_t *queue, void *item)
{
    item_t *_item = (item_t *)item;
    /* queue is empty */
    if (queue->head == NULL)
    {
        queue->head = item;
        queue->tail = item;
        _item->next = NULL;
        _item->prev = NULL;
    }
    else
    {
        ((item_t *)(queue->tail))->next = item;
        _item->next = NULL;
        _item->prev = queue->tail;
        queue->tail = item;
    }
}

//get head of the queue


/* remove this item and return next item */
void *queue_remove(queue_t *queue, void *item)
{
    item_t *_item = (item_t *)item;
    item_t *next = (item_t *)_item->next;

    if (item == queue->head && item == queue->tail)
    {
        queue->head = NULL;
        queue->tail = NULL;
    }
    else if (item == queue->head)
    {
        queue->head = _item->next;
        ((item_t *)(queue->head))->prev = NULL;
    }
    else if (item == queue->tail)
    {
        queue->tail = _item->prev;
        ((item_t *)(queue->tail))->next = NULL;
    }
    else
    {
        ((item_t *)(_item->prev))->next = _item->next;
        ((item_t *)(pcb->next))->prev = _item->prev;
    }

    _item->prev = NULL;
    _item->next = NULL;

    return (void *)next;
}

void timer_check()
{
    disable_preempt();

    /* TODO: check all timers
     *  if timeouts, call callback_func and free the timer.
     */
    pcb_t *p,*q;
    p = sleep_queue.head;
    uint64_t current=0;
    while(p!=NULL){
      current = get_timer();
      if(current - p->start_sleep >= p->sleep_time){
        p->status = TASK_READY;
        q=queue_remove(&sleep_queue,(void *)p);
        queue_push(p->ready_queue,(void *)p);
        p=q;
      }else
        p = p->next;
    }
    enable_preempt();
}

void measure_cost(uint64_t time){
    vt100_move_cursor(1,9);
    screen_cursor_x = 1;
    screen_cursor_y = 9;
    printk("> [TEST] Time cost of do_scheduler is %ld ticks.\n\r",time);
    vt100_move_cursor(current_running->cursor_x,current_running->cursor_y);
    screen_cursor_x = current_running->cursor_x;
    screen_cursor_y = current_running->cursor_y;   
}
/*void prior(void){
    int rand = time_elapsed%5;
    switch(rand){
        case 0:
            current_queue = &ready_queue[0];return;
        case 1:
            current_queue = &ready_queue[0];return;
        case 2:
            current_queue = &ready_queue[1];return;
        case 3:
            current_queue = &ready_queue[1];return;
        case 4:
            current_queue = &ready_queue[2];return;
    }
    if(!queue_is_empty(current_queue))
      current_running=(pcb_t *)queue_dequeue(current_queue);
    else
      current_queue = old;
    current_running->status=TASK_RUNNING;
}*/
//uint64_t *sched_cost;
uint32_t scheduler(void)
{
    // Modify the current_running pointer.
    //timer_check();
    int time_elapsed = get_ticks();
    int rand=time_elapsed % 6;
    if(current_running->status!=TASK_BLOCKED){
      current_running->status = TASK_READY;
    if(current_running->pid!=1)
        queue_push(current_queue,(void *)current_running);
    }
    queue_t *old=current_queue;
    if( rand > 2)
      current_queue=&ready_queue[0];
    else if(rand > 0)
      current_queue=&ready_queue[1];
    else
      current_queue=&ready_queue[2];
    //current_queue=&ready_queue[0];
    //prior();
    if(!queue_is_empty(current_queue))
      current_running=(pcb_t *)queue_dequeue(current_queue);
    else
      current_queue = old;
    current_running->status=TASK_RUNNING;
    // restore the current_runnint's cursor_x and cursor_y
    //vt100_move_cursor(1,2);
    //screen_cursor_x = 1;
    //screen_cursor_y = 2;
    
    //printk("> [Test] Time cost of scheduler is %u ticks.\n\r",time_elapsed_1 - time_elapsed);
    vt100_move_cursor(current_running->cursor_x,
                      current_running->cursor_y);
    screen_cursor_x = current_running->cursor_x;
    screen_cursor_y = current_running->cursor_y; 
    //int time_elapsed_1 = get_ticks();
    //sched_cost= time_elapsed_1 - time_elapsed;

}

void do_sleep(uint32_t sleep_time)
{
    // TODO: sleep(seconds)
    // note: you can assume: 1 second = `timebase` ticks
    // 1. block the current_running
    // 2. create a timer which calls `do_unblock` when timeout
    // 3. reschedule because the current_running is blocked.
    current_running->status = TASK_BLOCKED;
    current_running->start_sleep = get_timer();
    current_running->sleep_time = sleep_time;
    queue_push(&sleep_queue,current_running);
    do_scheduler();
    
}

void do_block(queue_t *queue)
{
    // TODO: block the pcb task into the block queue
    current_running->status=TASK_BLOCKED;
    queue_push(queue,(void *)current_running);
    do_scheduler();
}

void do_unblock(queue_t *queue)
{
    // TODO: unblock the `pcb` from the block queue
    pcb_t *p;
    if(!queue_is_empty(queue)){
      p=(pcb_t *)queue_dequeue(queue);
      p->status=TASK_READY;
      queue_push(p->ready_queue,(void *)p);
    }
      
}
